/*********************************************************************\
*
*                  (c) lkc
*                  All Rights Reserved. lkc Confidential
*
\*********************************************************************/
/*********************************************************************\
*   Title                      :
*
*   Module Description         :
*
*   Author                     : lkc
*
*   Created                    : 2018-6-24
*
\*********************************************************************/


#include <chrono>
#include <QDebug>
#include <thread>

#include "UtilityTH.h"
#include "ImageDetect.h"
#include "V4l2Device.h"

#include <QThread>

//ImageDetect imgdetect;

//bool SortByM1( const std::vector<cv::Point> &v1, const std::vector<cv::Point> &v2)//注意：本函数的参数的类型一定要与vector中元素的类型一致
//{
//    return v1.size() > v2.size();//降序排列
//}

////判断一个轮廓是不是粘连了，信标的点在能看清的时候粘连，那么其旋转外接矩形的长宽比例会比较大
//int isTogether(std::vector<cv::Point>& contour)
//{
//    int isTogether = 0;
//    //clock_t time1=clock();
//    cv::RotatedRect rotate_rect =cv::minAreaRect(contour);           //轮廓的旋转外接矩形
//    //如果长宽太小了 判断不准的
//    if(rotate_rect.size.height<=4&&rotate_rect.size.width<=4)
//        return 0;
//    //长宽比例
//    double rate = 0;
//    if(rotate_rect.size.height>rotate_rect.size.width)
//        rate = rotate_rect.size.height/rotate_rect.size.width;
//    else
//        rate = rotate_rect.size.width/rotate_rect.size.height;

//    //clock_t time2=clock();
//    //cout<< "boundingRect time is: "<<static_cast<double>(time2-time1)/CLOCKS_PER_SEC*1000<<"ms"<<endl;
//    //cout<< "rate is: "<<rate<<endl;
//    //cout<< "contour.size() is: "<<contour.size()<<endl;

//    if(rate>1.5)    //长宽比例>1.5，说明
//        isTogether=1;
//    return isTogether;
//}

ImageDetect ImageDetect::m_Instance;

ImageDetect& ImageDetect::GetInstance()
{
    return m_Instance;
}

ImageDetect::ImageDetect()
{
    m_objectPoints.reserve(OBJECT_SIZE_MAX);
    m_objectPoints.clear();
    m_dstImage = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
}

//ImageDetect::~ImageDetect()
//{
//}

void ImageDetect::DetectThread()
{
//    std::thread(std::bind(&ImageDetect::Detect,this)).detach();
    std::thread(&ImageDetect::Detect,this).detach();
}

std::vector <cv::Point2f> ImageDetect::GetObjectPoints()
{
    m_objectPointsMtx.lock();
    std::vector <cv::Point2f> retult(m_objectPoints);
    m_objectPointsMtx.unlock();
    return retult;
}

void ImageDetect::Detect()
{
    static timeval timeNow = {0,0};
    while(true)
    {
        cv::Mat matFrame;
        v4l2_buffer buf;
        auto ret = v4lDev.GetFrame(buf, matFrame);
        if(!ret)
        {
            qDebug() <<"GetFrame fail";
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            continue;
        }

        auto timeLast = timeNow;
        timeNow = buf.timestamp;
        long long interval_msec = (long long)(timeNow.tv_sec-timeLast.tv_sec)*1000+(timeNow.tv_usec-timeLast.tv_usec)/1000;
//        printf("timeLast    tv_sec:%d,  tv_usec:%d\n", timeLast.tv_sec, timeLast.tv_usec);
//        printf("timeNow    tv_sec:%d,  tv_usec:%d\n", timeNow.tv_sec, timeNow.tv_usec);
//        printf("interval_msec is %lldms\n",interval_msec);
        if(interval_msec<=0)
        {
//            printf("buf data has not updated, wait for next time\n");
            v4lDev.ReleaseBuf(buf);
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            continue;
        }
//        printf("buf data has updated, start image detect\n");
//        auto startTime = std::chrono::system_clock::now();
        cv::Mat tmp;
        matFrame.copyTo(tmp);
        v4lDev.ReleaseBuf(buf);
        FrameProcess(tmp);

//        auto endTime = std::chrono::system_clock::now();
//        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime);
//        qDebug() <<"Frame Process cost" << static_cast<double>(duration.count())<< " us";
//        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
    }
}

void ImageDetect::FrameProcess(cv::Mat &img)
{    
//    ShowImage(img.clone());
    cv::blur(img, img, cv::Size(20, 20));//后期待优化
//    int g_nThresh = 20;
//    cv::Canny(img, img, g_nThresh, g_nThresh * 2, 3);

    cv::Mat tempImg;
//    //OSTU进行二值化处理
//    m_OSTU = cv::threshold(img, tempImg, 10, 255, cv::THRESH_OTSU);         //OSTU方式寻找最佳的阈值
//    printf("mOSTU: %d\n",m_OSTU);
//    if(m_OSTU<30)
//    {
//        cv::threshold(img, tempImg, 30, 255, cv::THRESH_BINARY);
//    }
//    else
//    {
//        cv::threshold(img, tempImg, m_OSTU, 255, cv::THRESH_BINARY);
//    }
    cv::threshold(img, tempImg, 25, 255, cv::THRESH_BINARY);


    std::vector<cv::Vec4i> hierarchy;
    std::vector<std::vector<cv::Point>> contours;
    auto startTime = std::chrono::system_clock::now();
    cv::findContours( tempImg, contours, hierarchy,cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE );
    auto endTime = std::chrono::system_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime);
//    qDebug() <<"findContours cost" << static_cast<double>(duration.count())<< " us";
    ContoursProcess(contours);
}

void ImageDetect::ContoursProcess(std::vector<std::vector<cv::Point>> &contours)
{
    auto iter = contours.begin();
    while (iter != contours.end())
    {
        if (iter->size() < 60)
        {
//            qDebug() <<"this contour size"<<iter->size()<<"so erase";
            iter = contours.erase(iter);//对于vector, erase会返回下一个iterator,
        }
        else
        {
            ++iter;
        }
    }
//    qDebug() <<"contours.size "<<contours.size();
    FindGravity(contours);
//    cv::Mat image = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
//    drawContours(image, contours, -1, cv::Scalar(255, 255, 0), 1, 8);
//    ShowImage(image);
}


void ImageDetect::FindGravity(const std::vector<std::vector<cv::Point>> &contours)
{
    m_objectPointsMtx.lock();
//    m_objectPoints.clear();
    unsigned int number = (contours.size()>OBJECT_SIZE_MAX)?OBJECT_SIZE_MAX:contours.size();
    m_objectPoints.resize(number);
    for(unsigned int i = 0; i < number; i++ )
    {
        unsigned int conSize = contours[i].size();
//        qDebug() <<"this contour size"<<conSize;
        float pointx =0,pointy=0;
        for(unsigned int j=0; j<conSize;j++)
        {
            pointx+=contours[i][j].x;
            pointy+=contours[i][j].y;
        }
        pointx = pointx/conSize;
        pointy = pointy/conSize;
        m_objectPoints.at(i) = cv::Point2f( pointx, pointy);
//        qDebug() <<"Gravity" <<i<< "Centres x:"<<pointx<<"y:"<<pointy;
//        qDebug() <<"Gravity" <<i<< "Centres x:"<<m_objectPoints.at(i).x<<"y:"<<m_objectPoints.at(i).y;
//        cv::Moments mu = cv::moments( contours[i]);
//        qDebug() <<"mu x:"<<static_cast<float>(mu.m10/mu.m00)<<"y:"<<static_cast<float>(mu.m01/mu.m00)<<"mu.m10"<<mu.m10<<"mu.m00"<<mu.m00<<"mu.m01"<<mu.m01;
    }
    m_objectPointsMtx.unlock();
}

void ImageDetect::ShowImage(const cv::Mat &image)
{
    m_dstImageMtx.lock();
    m_dstImage = image;
    m_dstImageMtx.unlock();
}

//int ImageDetect::FindWhitePoint(std::vector<std::vector<cv::Point> > &contours)
//{
//    cv::Mat tempImg;
//    m_originalImg.copyTo(tempImg);
//    //OSTU进行二值化处理
//    m_OSTU = cv::threshold(tempImg, m_temp_img, 10, 255, cv::THRESH_OTSU);         //OSTU方式寻找最佳的阈值
//    printf("mOSTU: %d\n",m_OSTU);
////    if(mOSTU<15)
////    {
////        cv::threshold(tempImg, tempImg, 15, 255, cv::THRESH_BINARY);
////    }
////    else
////    {
////        cv::threshold(tempImg, tempImg, mOSTU, 255, cv::THRESH_BINARY);
////    }
//    cv::threshold(tempImg, tempImg, 90, 255, cv::THRESH_BINARY);
//    //输出显示
////    temp_img.copyTo(m_outImg);
//    //轮廓检测寻找白点 以及其大概的位置
//    std::vector<cv::Vec4i> hierarchy;
//    cv::findContours( tempImg, contours, hierarchy,cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE );    //寻找边

//    printf("contours.size():%d\n",contours.size());
//    std::sort(contours.begin(),contours.end(),SortByM1);  //根据边的尺寸降序排列

//    return Preprocese(contours);
//}

//int ImageDetect::Preprocese(std::vector<std::vector<cv::Point> > &contours)
//{
//    std::vector<std::vector<cv::Point> > point_contours(contours);

////    //对提取的轮廓进行滤波处理，滤除噪声,剔除轮廓中的噪声（大小差别夸张的点）
////    noise_remove(point_contours);


//    //对每个白点进行分裂测试
//    int test_num=0;     //需要测试的轮廓
//    if(point_contours.size()>3)
//        test_num = 3;
//    else
//        test_num = point_contours.size();
//    for(int i=0;i<test_num;)
//    {
//        if(isTogether(point_contours[i])==1)
//        {
//            //前面的点有粘连
//            std::vector<std::vector<cv::Point> > div_contours;
//            //使用一开始二值化作为阈值处理
//            process_together(point_contours[i],m_OSTU,div_contours);//把粘连的点分割开来
//            //cout<<"----process_together---- \n";
//            //删除老的轮廓
//            std::vector<std::vector<cv::Point> >::iterator it = point_contours.begin()+i;
//            point_contours.erase(it);
//            for(unsigned int j=0;j<div_contours.size();j++)
//            {
//                point_contours.push_back(div_contours[j]);
//            }
//            test_num--;
//        }
//        else
//        {
//            i++;
//        }
//    }


////    ///计算白点的中心 如果点的个数超过了10个 由大到小排列来得到10个点
//    std::sort(point_contours.begin(),point_contours.end(),SortByM1);  //根据边的尺寸降序排列
//    int final_contours_num = 0;
//    if(point_contours.size()>10)
//        final_contours_num = 10;
//    else
//        final_contours_num = point_contours.size(); //选择由大到小的前十个


//    m_objectPointsMtx.lock();
//    m_pointInFrame.clear();
//    for(int i=0;i<final_contours_num;i++)
//    {
//        double pointx =0,pointy=0;
//        for(unsigned int j=0; j<point_contours[i].size();j++)
//        {
//            pointx+=point_contours[i][j].x;
//            pointy+=point_contours[i][j].y;
//        }
//        pointx = pointx/point_contours[i].size();
//        pointy = pointy/point_contours[i].size();

//        WhitePoint a_WhitePoint;
//        a_WhitePoint.x = pointx;
//        a_WhitePoint.y = pointy;

//        m_pointInFrame.push_back(a_WhitePoint);
//    }
//    m_objectPointsMtx.unlock();

//    //    mPointNum = final_contours_num; //返回找到的白点的个数
//    return final_contours_num;
//}


////剔除轮廓中的噪声（大小差别夸张的点）
//void ImageDetect::noise_remove(std::vector<std::vector<cv::Point> > &contours)
//{
//    for(unsigned int i=0;i<contours.size();i++)//加上最小轮廓面积的判断？就是轮廓不能太小，不然大轮廓分开出来的几个像素就会被误判
//    {
//        if(contours[i].size()<3) //筛除过小的轮廓
//            contours.pop_back(); //删除最后一个轮廓 该轮廓
//    }
//    int contours_num = contours.size();
//    if(contours_num<3)  ///太少点了不需要滤波
//        return;

//    //找到剩下轮廓的中位数
//    //cout<<"mid contours:"<<contours[contours_num/2].size()<<"\n";

//    //寻找中位数之后的断层，来进行噪点的筛选
//    int max_change=0;
//    int mc_index=0;
//    for( int i =contours_num/2;i<contours_num;i++)
//    {
//        int size_change = contours[i-1].size() - contours[i].size();
//        if(size_change>max_change &&size_change>10 &&contours[i].size()<20)
//        {
//            mc_index = i;
//            max_change = size_change;
//        }
//    }
//    if(mc_index==0)
//        return;
//    for(int i=mc_index;i<contours_num;i++) //后面的都删除
//    {
//        contours.pop_back(); //删除最后一个轮廓 该轮廓
//    }
//}


//void ImageDetect::process_together(std::vector<cv::Point>& contour,int th,std::vector<std::vector<cv::Point> >& div_contours) //0.1ms
//{
//    std::vector<std::vector<cv::Point> > together_contours;
//    //第一次的分裂
//    div_point_once(contour,th,div_contours,together_contours);
//    if(together_contours.size()>0)  //对每个没有分裂开来的轮廓
//    {
//        std::sort(together_contours.begin(),together_contours.end(),SortByM1);  //根据轮廓的尺寸降序排列
//        int count=0;
//        int step = 1;
//        for(int i=0;together_contours.size()>0&&count<10;)  //对于有粘连的轮廓进行10次的分裂二值化
//        {
//            //从粘连的轮廓中分裂
//            div_point_once(together_contours[i],th+step,div_contours,together_contours);
//            //删除老的轮廓
//            std::vector<std::vector<cv::Point> >::iterator it = together_contours.begin()+i;
//            together_contours.erase(it); //不能i++了
//            //cout<<"together_contours.size();"<<together_contours[i].size()<<"\n";
////            cout<<"step;"<<step<<"\n";

//            if(together_contours[i].size()<20/3)
//                step+=together_contours[i].size()/3;
//            else
//                step+=20/3;
//            count++;
//        }
//    }
//}


//int ImageDetect::div_point_once(std::vector<cv::Point>& contour,int th,std::vector<std::vector<cv::Point> >& div_contours,std::vector<std::vector<cv::Point> >& together_contours)
//{
//    if(th >= 255)//防止th超过限制
//        th =255;

//    //提取包含轮廓的最小矩形
//    cv::Rect mrect=boundingRect(contour); //mrect.x mrect.y 就是坐标的偏移量

//    if(mrect.width*mrect.height<9)  //too small
//        return 0;
//    int offset=3;   //超出边沿多少像素
//    int off_x = 0,off_y=0;
//    if(mrect.x-offset>0)
//        off_x = mrect.x-offset;
//    else
//        off_x = 1;
//    if(mrect.y-offset>0)
//        off_y = mrect.y-offset;
//    else
//        off_y = 1;

//    int width_x =0,width_y=0;
//    if(off_x+mrect.width+2*offset>655)
//        width_x = 655-off_x;
//    else
//        width_x = mrect.width+2*offset;
//    if(off_y+mrect.height+2*offset>491)
//        width_y = 491-off_y;
//    else
//        width_y = mrect.height+2*offset;

//    cv::Mat pro_img = m_originalImg(cv::Rect(off_x,off_y,width_x,width_y)).clone();//rect 起始点，长宽

////    cv::Mat m_contoursImg;// = Mat::zeros(pro_img.rows,pro_img.cols,CV_8UC1); //纯黑的图
//    cv::threshold(pro_img, m_contoursImg, 255, 255, cv::THRESH_BINARY); //纯黑图像
//    for(unsigned int i=0;i<contour.size();i++)   //轮廓在当前小图中的坐标       ///有点耗时
//    {
//        contour[i].x -=off_x;
//        contour[i].y -=off_y;
//    }

//    std::vector<std::vector<cv::Point> > contours1;
//    contours1.push_back(contour);
//    cv::drawContours(m_contoursImg,contours1,0,cv::Scalar(255),cv::FILLED);       ///耗时的地方 0.06ms 要不改成矩形？
//    cv::bitwise_and(m_contoursImg, pro_img,m_contoursImg, cv::noArray());        //提取出轮廓的区域

////    cv::namedWindow("process_together_before", CV_WINDOW_NORMAL);  //防止图像太小看不到
////    cv::imshow("process_together_before",temp_img);    //显示新二值化的轮廓

//    //使用更高的阈值二值化，再次检测轮廓，如果分开了就返回1，如果没分开返回0
//    cv::threshold(m_contoursImg, m_contoursImg, th, 255, cv::THRESH_BINARY); //再次二值化

//    //轮廓检测寻找白点 以及其大概的位置
//    std::vector<std::vector<cv::Point> > contours;
//    std::vector<cv::Vec4i> hierarchy;
//    cv::findContours( m_contoursImg.clone(), contours, hierarchy,cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE );    //寻找边
//    std::sort(contours.begin(),contours.end(),SortByM1);  //根据边的尺寸降序排列
/////0.03ms

//    for(unsigned int i=0;i<contours.size();i++)
//    {
//        cv::drawContours(m_contoursImg,contours,0,cv::Scalar(255),cv::FILLED);
//    }
////    namedWindow("process_together_after", CV_WINDOW_NORMAL);  //防止图像太小看不到
////    imshow("process_together_after",temp_img);    //显示新二值化的轮廓

//    //判断分开的轮廓是否粘连
//    for(unsigned int i=0;i<contours.size();i++)
//    {
//        if(isTogether(contours[i])==0) //没有粘连
//        {
//            for(unsigned int j=0;j<contours[i].size();j++)   //返回复原的图片
//            {
//                contours[i][j].x +=off_x;
//                contours[i][j].y +=off_y;
//            }
//            div_contours.push_back(contours[i]);    //输出到返回序列中
//        }
//        else    //有粘连 返回到有粘连的序列中
//        {
//            for(unsigned int j=0;j<contours[i].size();j++)   //返回复原的图片
//            {
//                contours[i][j].x +=off_x;
//                contours[i][j].y +=off_y;
//            }
//            together_contours.push_back(contours[i]);    //输出到返回序列中
//        }
//    }

//    return 1;

//}

/********************************************************************\
 *
 * REVISION RECORDS
 *
\*********************************************************************/
/*********************************************************************/
/*
 *
 *
 *
\*********************************************************************/
/*------------------------------The End------------------------------*/
